#include <chrono>
#include <cmath>
#include <fstream>  // 文件流操作
#include <iostream>
#include <vector>

#include "common/data.h"
#include "common/path.h"
#include "gpu/spatical_sampling.h"

std::vector<RefPoint> create_refline() {
    std::vector<RefPoint> refline;

    float R = 100;
    float theta1 = M_PI / 2;
    float thata2 = 0;
    float thete_reso = kRefPointResolution / R;

    float x_offset = R;
    float y_offset = 0;

    auto get_refpoint = [&](float theta, RefPoint* ref_point) {
        float temp_s = std::abs(theta - theta1) * R;
        float temp_y = y_offset + R * std::sin(theta);
        float temp_x = x_offset + R * std::cos(theta);
        float temp_theta = theta - M_PI / 2.0;

        ref_point->s = temp_s;
        ref_point->x = temp_x;
        ref_point->y = temp_y;
        ref_point->theta = temp_theta;
    };

    float curr_theta = theta1;
    while (curr_theta >= thata2) {
        RefPoint ref_point;
        get_refpoint(curr_theta, &ref_point);
        refline.push_back(ref_point);
        curr_theta -= thete_reso;
    }

    return refline;
}

std::vector<Path> sample_path(const std::vector<RefPoint>* refline) {  // 对path进行采样
    std::vector<Path> path_vec;

    float s0 = 0;
    float l0 = 0;
    float theta0 = 0;
    PathPoint path_point0(s0, l0, theta0);

    float s1 = 25;
    float l1_min = l0 - 1.5;
    float l1_max = l0 + 1.5;
    float l1_delta = 0.5;

    float s2 = 50;
    float l2_min = l0 - 2.5;
    float l2_max = l0 + 2.5;
    float l2_delta = 0.6;

    float s3 = 100;
    float l3_min = l0 - 3.75;
    float l3_max = l0 + 3.75;
    float l3_delta = 0.8;

    int path_count = 0;
    std::vector<PathPoint> path_point_vec(4, path_point0);

    float l1 = l1_max;
    while (l1 >= l1_min) {
        PathPoint path_point1(s1, l1, theta0);
        path_point_vec[1] = path_point1;

        // float l2 = std::min(l1, l2_max);
        float l2 = l2_max;
        while (l2 >= l2_min) {
            PathPoint path_point2(s2, l2, theta0);
            path_point_vec[2] = path_point2;

            // float l3 = std::min(l2, l3_max);
            float l3 = l3_max;
            while (l3 >= l3_min) {
                PathPoint path_point3(s3, l3, theta0);
                path_point_vec[3] = path_point3;
                path_vec.emplace_back(path_count++, 20, refline, path_point_vec);
                l3 -= l3_delta;
            }
            l2 -= l2_delta;
        }
        l1 -= l1_delta;
    }
    return path_vec;
}

int main() {
    std::vector<RefPoint> refline = create_refline();
    std::cout << "ref point nums: " << refline.size() << std::endl;

    std::ofstream out_file0("refline.txt");
    assert(out_file0.is_open());
    for (int i = 0; i < refline.size(); i++) {
        auto& ref_point = refline[i];
        out_file0 << "(" << ref_point.x << ", " << ref_point.y << "), ";
    }
    out_file0.close();

    std::vector<Path> path_vec = sample_path(&refline);
    std::cout << "path nums: " << path_vec.size() << std::endl;

    // 测试在CPU上采样的耗时
    for (int i = 0; i < 1; i++) {
        auto start1 = std::chrono::high_resolution_clock::now();
        for (int i = 0; i < path_vec.size(); i++) {
            path_vec[i].sampling_on_cpu(0.2);
        }
        auto end1 = std::chrono::high_resolution_clock::now();
        auto duration1 = std::chrono::duration_cast<std::chrono::microseconds>(end1 - start1);
        std::cout << "sampling_on_cpu 耗时: " << duration1.count() << " 微秒" << std::endl;
    }

    // 将CPU上采样的结果dump到文件
    std::ofstream out_file("path_output_cpu.txt");
    assert(out_file.is_open());
    for (int i = 0; i < path_vec.size(); i++) {
        path_vec[i].dump(out_file, 0.2);
    }
    out_file.close();

    // 测试初始化内存的耗时, 该内存将来会被拷贝到GPU上
    SpaticalSampler spatical_sampler;
    int used_cpu_memory_size = 0;
    for (int i = 0; i < 1; i++) {
        auto start1 = std::chrono::high_resolution_clock::now();
        used_cpu_memory_size = spatical_sampler.init_memory(refline, path_vec);
        auto end1 = std::chrono::high_resolution_clock::now();
        auto duration1 = std::chrono::duration_cast<std::chrono::microseconds>(end1 - start1);
        std::cout << "init_memory 耗时: " << duration1.count() << " 微秒" << std::endl;
    }
    std::cout << "total cpu memory: " << used_cpu_memory_size / 1024.0 << "kB" << std::endl;

    // 测试在GPU上采样的耗时
    for (int i = 0; i < 10; i++) {
        auto start2 = std::chrono::high_resolution_clock::now();
        spatical_sampler.sampling_on_gpu(used_cpu_memory_size);
        auto end2 = std::chrono::high_resolution_clock::now();
        auto duration2 = std::chrono::duration_cast<std::chrono::microseconds>(end2 - start2);
        std::cout << "sampling_on_gpu 耗时: " << duration2.count() << " 微秒" << std::endl;
    }

    // spatical_sampler.debug_print(false);
    std::cout << "PointInfo size: " << sizeof(PointInfo) << std::endl;
    // spatical_sampler.debug_print(true);

    // 将GPU上采样的结果dump到文件
    std::ofstream out_file2("path_output_gpu.txt");
    assert(out_file2.is_open());
    spatical_sampler.dump(out_file2, true);
    out_file2.close();

    return 0;
}